import {
    Node,
    SequenceNode,
    ChooseNode,
    ParallelNode,
    InversionDecroator,
    ForDecroator,
} from "./BehaviorTree";

import {
    makeLine,
    makeCircle,
    makeNavigationControl,
    makeNewNavPointControl,
    getObjectById,
    ConnectPointAction,
    DjPoint,
    findMinDistancePoint,
    checkInList,
} from "./util";


/* 
使用了 “行为树” 的设计模式，可百度了解一下 Behavior Tree 的编程模式
*/

// 找到最近的点
class FindNearestPoint extends Node {
    constructor(name, blackboard) {
        super(name)
        this.blackboard = blackboard

    }
    async tick() {
        let that = this

        return new Promise((resolve, reject) => {
            var blackboard = this.blackboard
            let machine = blackboard.machine
            let points = blackboard.points
            // 拿到起始点数据，迭代所有点，找出最近点。 导航到时近点。然后这一点找最近点，导航过去，直到终点。
            let findResult = findMinDistancePoint(machine, points);
            var minDistance = findResult.minDistance; //最小距离
            var startPoint = findResult.targetPoint; //找到的距离更小的点
            this.blackboard.startPoint = startPoint

            setTimeout(() => {
                let move_sj = window.topics.getTopic('/' + this.name + '/move')
                move_sj.next({ id: that.id, speed: that.blackboard.speed, target: findResult.targetPointId })
                resolve(1)
            }, 500);

        });

    }
}


//全局规划器
class GlobalPlanner extends Node {
    constructor(name, blackboard) {
        super(name)
        this.blackboard = blackboard
    }
    async tick() {
        let that = this
        var blackboard = this.blackboard
        this.startPoint = blackboard.startPoint
        this.target = blackboard.target
        try {
            var result = this.doDijkstra(this.startPoint, this.target)
            console.log('blackboard result', result);
            this.blackboard['routes'] = result.routes //将结果写入黑板
            if (result) {
                return 1
            } else {
                return 0
            }
        } catch (error) {
            return 0
        }
    }
    /***
    获取起点坐标、目标点坐标，把起点放进openlist
    while (openlist不为空列表)
    {	取openlist中路程最小的点为中心点，从openlist剔除，放入closedlist
        if (中心点是目标点)
        {从目标点开始通过父节点反推，提取出路径，break跳出循环}
        else {遍历相邻的点
            {if(点已存在closedlist中  or  遇到障碍物 )
            {跳过，不计算这个点}
            else
            {累计路径
            if(点已存在openlist中)
                if(累计路程<原来记录的路程)
                {替换数据}
            else {加入openlist}
            }
        openlist从小到大排序
        }
    }
    ***/
    doDijkstra(start, end) {
        console.log("endid,", end.id);
        var resultRoutes;

        var openlist = [];
        var closedlist = [];

        let startPoint = new DjPoint(start.id);
        startPoint.routes.push(start.id);

        openlist.push(startPoint);
        while (openlist.length > 0) {
            openlist.distanceBubbleSort();
            // 取出中心点！
            var centerPoint = openlist.shift();
            console.log("取出中心点！");
            closedlist.push(centerPoint);
            // console.log("centerPoint.id,", centerPoint.id);
            // console.log("end.id,", end.id);
            if (centerPoint.id == end.id) {
                //到达终点
                // console.log("到达终点");
                resultRoutes = centerPoint;
                break;
            } else {
                //遍历中心点相邻的点
                // console.log("遍历中心点相邻的点");
                var lines = centerPoint.point.lines;
                for (let i = 0; i < lines.length; i++) {
                    const id = lines[i];
                    var line = getObjectById(id);
                    var point;
                    if (line.start == centerPoint.point.id) {
                        point = getObjectById(line.end);
                    } else if (line.end == centerPoint.point.id) {
                        point = getObjectById(line.start);
                    }

                    if (checkInList(point, closedlist).result) {
                        //跳过
                        // console.log("跳过");
                    } else {
                        let newPoint = new DjPoint(point.id);
                        // console.log("计算距离");
                        newPoint.jointPoint(centerPoint); //计算距离
                        let checkOpenlistResult = checkInList(newPoint, openlist);
                        if (checkOpenlistResult.result) {
                            if (
                                // 累计路程<原来记录的路程
                                newPoint.distance <
                                openlist[checkOpenlistResult.index].distance
                            ) {
                                // console.log("累计路程<原来记录的路程 替换数据");
                                //替换数据
                                openlist[checkOpenlistResult.index].distance =
                                    newPoint.distance;
                            }
                        } else {
                            // 将新点加入openlist 待重新评估
                            // console.log("将新点加入openlist 待重新评估");
                            openlist.push(newPoint);
                        }
                    }
                }
            }
        }
        console.log("Dijkstra完成，", resultRoutes);
        return resultRoutes;
    }
}

//执行监控器
class LocalPlanner extends Node {
    constructor(name, blackboard) {
        super(name)
        this.blackboard = blackboard
    }
    init() {
        let blackboard = this.blackboard
        this.startPoint = blackboard.startPoint
        this.target = blackboard.target
        this.routes = blackboard.routes
        this.finishedNodeCount = 0 //完成节点的计数
        let sprList = []
        let routes = this.routes
        for (let i = 0; i < (routes.length - 1); i++) {
            const route = routes[i];
            const routeNext = routes[i + 1];
            let bb = JSON.parse(JSON.stringify(blackboard))
            bb['index'] = i
            let spr = new SinglePathRunner(this.name, route, routeNext, bb)
            sprList.push(spr)
        }
        this.sprList = sprList
    }
    async tick() {
        let that = this
        let count = { count: 0 }

        return new Promise((resolve, reject) => {
            if (that.nodeStatus == -1) {
                that.init() //进行必要的初始化
                that.nodeStatus = 2
                let sprList = that.sprList
                console.log("count.count", count.count);
                that.sprList[count.count].tick()
                let completeFn = function (msg) {
                    that.blackboard.current = msg.current
                    if (msg.count == sprList.length) {
                        that.nodeStatus = 1
                        resolve(that.nodeStatus)
                    } else {
                        setTimeout(() => {
                            that.sprList[count.count].tick()
                        }, 500);
                    }
                }

                for (let i = 0; i < sprList.length; i++) {
                    const sprSubject = sprList[i];
                    sprSubject.resultSj.subscribe({
                        complete(msg) {
                            count.count++
                            completeFn({ count: count.count, current: msg })
                        },
                        error: function (param) {
                            that.nodeStatus = 0
                            reject(that.nodeStatus)
                        }
                    })
                }
            }
        });


    }
}

class SinglePathRunner extends Node {
    constructor(name, startPoint, target, blackboard) {
        super(name)
        this.startPoint = startPoint
        this.target = target
        this.routes = blackboard.routes
        this.speed = blackboard.speed
        this.index = blackboard.index
        this.machine = blackboard.machine

        this.resultSj = new Rx.Subject();
    }
    async tick() {
        let that = this

        if (this.nodeStatus == -1) {
            console.log(that.name + 'ticked!!! +++++++++++');
            //TODO 后续改成带确认的服务机制
            let move_sj = window.topics.getTopic('/' + this.name + '/move')
            move_sj.next({ id: that.id, speed: that.speed, target: that.target })

            let position_sj = window.topics.getTopic('/' + this.name + '/position')
            position_sj.subscribe({
                next: function (msg) {
                    // 确认小车位置已经到达弱点附近
                    // TODO for test ,目前只要收到信号就当已经到达，后续修改成带判断逻辑,或者添加一些需要招待的任务，才能完成这个导航
                    that.nodeStatus == 1
                    that.resultSj.complete(that) //完成节点，resultSj 不能再调用 next()
                },
                error: function (param) {
                    that.resultSj.error(param)
                }
            })
            // TODO 后续添加超时机制
            this.nodeStatus == 2
        }
        return this.resultSj
    }
}

class DoTaskSequenceNode extends SequenceNode {
    constructor(name, nodeList, blackboard, target) {
        super(name, nodeList);
        blackboard.target = target
        this.blackboard = blackboard
        this.genNodeList(blackboard)
    }
    genNodeList() {
        let blackboard = this.blackboard
        let id = this.id
        let fnp = new FindNearestPoint(id, blackboard) // 找到最近的二维码
        let gp = new GlobalPlanner(id, blackboard) // 全局规划
        let lp = new LocalPlanner(id, blackboard) // 局部规划 （走完计算出来的所有点）
        this.nodeList.push(fnp)
        this.nodeList.push(gp)
        this.nodeList.push(lp)
    }
}


let doNavTask = async function (blackboard) {
    let id = blackboard.machine.id //小车id

    // 取食材
    let s1 = new DoTaskSequenceNode(id, [], JSON.parse(JSON.stringify(blackboard)), getObjectById("18f026fbad599a4622a8395a541f3d55"))
    // 放到食物柜煮食物
    let s2 = new DoTaskSequenceNode(id, [], JSON.parse(JSON.stringify(blackboard)), getObjectById("95e9a53f1c3f9143dd1899d10a29fb0e"))
    // 将食物放到快递柜
    let s3 = new DoTaskSequenceNode(id, [], JSON.parse(JSON.stringify(blackboard)), getObjectById("1c8ef2a389033844718894e831feed5b"))
    // 自动回到充电器充电
    let s4 = new DoTaskSequenceNode(id, [], JSON.parse(JSON.stringify(blackboard)), getObjectById("d53e3a9fe01a1e4bb1e8843e07d10b75"))

    // 使用行为树的 顺序节点 执行全部过程
    let sq = new SequenceNode(id, [
        s1, s2, s3, s4
    ])
    await sq.tick()

}
export { doNavTask }